#!/usr/bin/env python
#-*- coding:UTF-8 -*-
import rospy
from std_msgs.msg import UInt8
import gpio_ctrl as io
import Jetson.GPIO as GPIO
import time

check_over = 0

# 回调：读取传感器连接状态消息，对应不同提示灯
def uav_cb(msg):
    tmp =0
    if msg.data == 1:
        io.gpio_led_off(io.led_ch1)
        io.gpio_led_off(io.led_ch2)
        io.gpio_led_off(io.led_ch3)
        io.gpio_led_off(io.led_ch4)
        io.gpio_beep_ctrl(io.beep_ch)
        tmp = 1        

    if msg.data %2 == 0:
        io.gpio_led_on(io.led_ch1)
	io.gpio_led_off(io.led_ch2)
        io.gpio_led_off(io.led_ch3)
        io.gpio_led_off(io.led_ch4)
    if msg.data %3 == 0:
        io.gpio_led_on(io.led_ch2)
        io.gpio_led_off(io.led_ch1)
        io.gpio_led_off(io.led_ch3)
        io.gpio_led_off(io.led_ch4)
    if msg.data %5 == 0:
	io.gpio_led_off(io.led_ch1)
        io.gpio_led_off(io.led_ch2)
        io.gpio_led_off(io.led_ch4)
        io.gpio_led_on(io.led_ch3)
    if msg.data %7 == 0:
	io.gpio_led_off(io.led_ch1)
        io.gpio_led_off(io.led_ch2)
        io.gpio_led_off(io.led_ch3)
        io.gpio_led_on(io.led_ch4)
    if msg.data %11== 0:
        io.gpio_led_on(io.led_ch1)
        io.gpio_led_on(io.led_ch2)
        io.gpio_led_on(io.led_ch3)
        io.gpio_led_on(io.led_ch4)
    
    #GPIO.cleanup()
    return tmp


# 检查传感器，循环执行回调直至全部连接
def check_sensors():
    check_over = 0
    while (not rospy.is_shutdown()) and (not check_over==1):
        msg = rospy.wait_for_message("/uav/connected", UInt8, timeout=None)
        check_over = uav_cb(msg)
    GPIO.cleanup()

# 等待起飞指令，即按钮按下
def preflight():
    print("wait!")
    pub = rospy.Publisher("/uav/start", UInt8, queue_size=10)
    rate = rospy.Rate(20)
    
    io.gpio_button_ctrl(io.but_ch)

    # 蜂鸣器报警提醒
    for i in range(25):
        io.gpio_beep_ctrl(io.beep_ch, 0.1)
        time.sleep(0.1)
    msg = 1

    # 发布主程序启动指令
    while not rospy.is_shutdown():
        pub.publish(msg)
        rate.sleep()



if __name__ == '__main__':
    rospy.init_node('check', anonymous=True)
    GPIO.cleanup() 

    GPIO.setwarnings(False)
    check_sensors()
    preflight()
